/*
 * long_adc.c
 *
 *  Created on: 2021年3月7日
 *      Author: HP
 */

/*
 * sc_adc.c
 *
 *  Created on: 2021年2月27日
 *      Author: SYX
 */

#include "long_adc.h"

//数据处理的flag
uint8 process_flag = 0;
uint8 nnom_process_flag = 1;
uint8 short_process_flag = 0;
uint8 send_data_flag=0;
int8 round_num=0;

//用于控制的flag
uint8 cross_pid_change_flag=0;


//AI采数数组和控制采数数组分开


float AD[(AD_NUM+AI_NUM)];
float MinLVGot=0.01;
float err_ad_now[2]={0,0};
float err_round_ai=0;
float err_45_curve_ai=0;
float err_45_curve_ai_last=0;
float err_round_ai_last=0;
float err_synthetical_now = 0;
float num_of_three_horizen = 0;
float intergration = 0.0;
float round_achive_angle=0.0;
//static float Max[(AD_NUM+AI_NUM)];
_Bool Flag_Find_Max = 0;//一开始不开始寻找
uint8 If_Start = 0;//延时发车
uint8 channel_name[AD_NUM]={ADC2_CH6_A38,ADC0_CH13_A13,ADC2_CH3_A35,ADC0_CH12_A12,ADC2_CH4_A36,ADC1_CH1_A17,ADC1_CH5_A21};
uint8 channel_adc[AD_NUM]={ADC_2,ADC_0,ADC_2,ADC_0,ADC_2,ADC_1,ADC_1};
//使AD通道 0到6分别对应 左1横、右一横、左竖、右竖、左二横、右二横、中间电感

//AI电感采数数组
uint8 channel_AI[AI_NUM]={ADC0_CH10_A10,ADC0_CH11_A11,ADC0_CH8_A8,ADC0_CH7_A7,ADC0_CH6_A6,ADC0_CH5_A5,ADC0_CH4_A4,ADC0_CH3_A3,ADC0_CH2_A2,ADC0_CH1_A1,ADC2_CH12_A46,ADC2_CH7_A39,ADC2_CH10_A44,ADC2_CH5_A37};
uint8 channel_AI_adc[AI_NUM]={ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_0,ADC_2,ADC_2,ADC_2,ADC_2};
static float Average[AD_NUM];
static float Average_AI[AI_NUM];
//typedef enum
//{
//    //ADC0可选引脚
//    ADC0_CH0_A0   = 0*16 + 0,
//    ADC0_CH1_A1,
//    ADC0_CH2_A2,
//    ADC0_CH3_A3,
//    ADC0_CH4_A4,
//    ADC0_CH5_A5,
//    ADC0_CH6_A6,
//    ADC0_CH7_A7,
//    ADC0_CH8_A8,
//    ADC0_CH10_A10 = 0*16 + 10,
//    ADC0_CH11_A11,
//    ADC0_CH12_A12,
//    ADC0_CH13_A13,
//
//    //ADC1可选引脚
//    ADC1_CH0_A16  = 1*16 + 0,
//    ADC1_CH1_A17  = 1*16 + 1,
//    ADC1_CH4_A20  = 1*16 + 4,
//    ADC1_CH5_A21  = 1*16 + 5,
//    ADC1_CH8_A24  = 1*16 + 8,
//    ADC1_CH9_A25  = 1*16 + 9,
//
//    //ADC2可选引脚
//    ADC2_CH3_A35  = 2*16 + 3,
//    ADC2_CH4_A36,
//    ADC2_CH5_A37,
//    ADC2_CH6_A38,
//    ADC2_CH7_A39,
//    ADC2_CH10_A44 = 2*16 + 10,
//    ADC2_CH11_A45,
//    ADC2_CH12_A46,
//    ADC2_CH13_A47,
//    ADC2_CH14_A48,
//    ADC2_CH15_A49,
//}VADC_CHN_enum;

//上位机传回的数组
uint8 type_of_road = 0;
uint8 send_ai_ad = 1;
uint8 into_the_ring=0;
uint8 right_round=0;
uint8 left_round=0;
uint8 out_the_ring=0;
uint8 in_the_round=0;
uint8 up_speed_flag=1;
int8 ai_data[AI_NUM]={0};
uint8 pid_round_process=0;
uint16 Repeated_recognition_flag = 900;

//static uint8 round_flag=0;


uint8 collect_data_finish=0;//1：ad数据采集完成   0：ad数据未采集完成
uint8 collect_max_flag = 0;
uint8 send_buff[SendDataTime];//WIFI传adc数据
uint8 send_test[17];//WIFI传adc数据
float Max[(AD_NUM+AI_NUM)]={0};
//static uint32 Save_max[(AD_NUM+AI_NUM)];
//int16 right_threshould=15;
//int16 cancel_right_ad=115;
//int16 _SCFTM=1000;

//传统控制的算法阈值判断结构体
Judgment_threshold long_theshold={
        .cancel_90_status= 0 ,

        };
Judgment_threshold short_theshold={
        .cancel_90_status= 0 ,
        .encoder_90_num=0,
        .is_cross_not_90=0,
        .turn_90_curve=0,
        .is_round_not_90_mid=0,
        };
Judgment_threshold AI_theshold;

ANGLE_CONTROL AI_angle;

void swap(uint16 *a,uint16 *b)
{
    uint16 temp=*a;
    *a=*b;
    *b=temp;
}




void Long_AD_Sample(void)
{
    uint16 LV_control[AD_NUM][SampleTimes];
    float transfer[AD_NUM]={0};
    for (uint8 h=0;h<AD_NUM;h++)
        {
            for(uint8 i=0;i<=SampleTimes-1;i++)
            {
             /*获取采样初值*/
#if     defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==12U)
                LV_control[h][i] = ADC_Get(channel_adc[h], channel_name[h], ADC_12BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==10U)
                LV_control[h][i] = ADC_Get(channel_adc[h], channel_name[h], ADC_10BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==8U)
                LV_control[h][i] = ADC_Get(channel_adc[h], channel_name[h], ADC_8BIT);
#endif
            }
        }

    for(uint8 i=0;i<AD_NUM;i++)
    {
        for(uint8 j=0;j<=SampleTimes-1;j++)
        {
             if(LV_control[i][j]>BIT_MAX)//剔除毛刺信号
             {
                 LV_control[i][j]=BIT_MAX;
             }
        }
    }
    for(uint8 k=0;k<AD_NUM;k++)
    {
        for(uint8 i=0;i<=SampleTimes-2;i++)
        {
            for(uint8 j=i+1;j<=SampleTimes-1;j++)
            {
                if(LV_control[k][i]>LV_control[k][j])
                swap(&LV_control[k][i],&LV_control[k][j]);//������swap�����Լ�д
            }
        }
    }
    for(uint8 k=0;k<AD_NUM;k++)
    {
        for(uint8 i=3;i<SampleTimes-3;i++)
        {
            transfer[k]+=(float)((int16)LV_control[k][i]);
        }
        Average[k]=transfer[k]/(SampleTimes-6.0);
        if( Average[k] < MinLVGot )
        {
            Average[k] = MinLVGot;
        }
    }
}

void Short_AD_Sample(void)
{
    float transfer_AI[AI_NUM]={0};
    uint16 LV_AI[AI_NUM][SampleTimes_AI];
    for (uint8 h=0;h<AI_NUM;h++)
    {
        for(uint8 i=0;i<=SampleTimes_AI-1;i++)
        {
#if     defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==12U)
            LV_AI[h][i] = ADC_Get(channel_AI_adc[h], channel_AI[h], ADC_12BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==10U)
            LV_AI[h][i] = ADC_Get(channel_AI_adc[h], channel_AI[h], ADC_10BIT);
#elif   defined   (INDUCTANCE_ACCURACY) &&  (INDUCTANCE_ACCURACY==8U)
            LV_AI[h][i] = ADC_Get(channel_AI_adc[h], channel_AI[h], ADC_8BIT);
#endif
        }
    }

    for(uint8 i=0;i<AI_NUM;i++)
           {
               for(uint8 j=0;j<SampleTimes_AI;j++)
               {
                    if(LV_AI[i][j]>BIT_MAX)//剔除毛刺信号
                    {
                        LV_AI[i][j]=BIT_MAX;
                    }
               }
           }
    for(uint8 k=0;k<AI_NUM;k++)
    {
        for(uint8 i=0;i<=SampleTimes_AI-2;i++)
        {
            for(uint8 j=i+1;j<=SampleTimes_AI-1;j++)
            {
                if(LV_AI[k][i]>LV_AI[k][j])
                swap(&LV_AI[k][i],&LV_AI[k][j]);
            }
        }
    }
    for(uint8 k=0;k<AI_NUM;k++)
          {
               for(uint8 i=3;i<=SampleTimes-4;i++)
               {
                   transfer_AI[k]+=(float)((int16)LV_AI[k][i]);
               }
               Average_AI[k]=transfer_AI[k]/(SampleTimes_AI-6.0);
               if( Average_AI[k] < MinLVGot )
               {
                  Average_AI[k] = MinLVGot;
               }
           }

}

void Check_Round(void)
{

//    if(AD[12]>AI_theshold.round_midjudge&&Repeated_recognition_flag>800)
//    {
//        pid_round_process=1;
//    }
//    if(AD[11]<AI_theshold.out_road_flag_theshold)
//    {
//        out_road_flag=1;
//    }
//    else
//    {
//        out_road_flag=0;
//    }

}

void round_process(void)
{
//    float AIMid_ad=AD[11];
//    float AIL_horizontal=AD[8];
//    float AIR_horizontal=AD[14];
//    if (in_the_round==0)
//    {
//       type_of_road=30;
////       acc_encoder=0;
//    }
//    if(type_of_road==30)
//    {
//        if(AIMid_ad>AI_theshold.into_round)
//        {
//            if(right_round==0&&left_round==0)
//            {
//                if(AIL_horizontal>AIR_horizontal)
//                {
//                    left_round=1;
//                    right_round=0;
//                    intergration = 0.0f;
//                }
//                else
//                {
//                    right_round=1;
//                    left_round=0;
//                    intergration = 0.0f;
//                }
//            }
//            into_the_ring=1;
//        }
//    }
//    if(type_of_road==30&&into_the_ring==1)
//    {
//        if(intergration>=AI_theshold.angle||intergration<=-AI_theshold.angle)
//        {
////            type_of_road=0;//环内是用AI去控制还是传统区去控制
//            pid_round_process = 0;
//            into_the_ring=0;
////                right_round=0;
////                left_round=0;
//            in_the_round=1;
//        }
//    }
//    if(in_the_round==1)
//    {
//
//          type_of_road=31;
//          acc_encoder=0;
//
//    }
//    if(type_of_road==31&&AIMid_ad>AI_theshold.out_flagjudge)
//    {
//         if(out_the_ring==0)
//         {
//             intergration = 0.0f;
//         }
//        out_the_ring=1;
//    }
//
//    if(out_the_ring==1&&type_of_road==31&&(intergration>=AI_theshold.angle_out||intergration<=-AI_theshold.angle_out))
//    {
//            pid_round_process = 0;
//            in_the_round=0;
//            right_round=0;
//            left_round=0;
//            out_the_ring=0;
//            Repeated_recognition_flag = 0;
//    }
}

//if(in_the_round==0&&SMid_ad>short_theshold.round_midjudge)
//{
//
//      type_of_road=30;
//      acc_encoder=0;
//
//}
//if(type_of_road==30)
//{
//    if(SMid_ad>short_theshold.into_round)
//    {
//        into_the_ring=1;
//        if(right_round==0&&left_round==0)
//        {
//            if(SL_horizontal>SR_horizontal)
//            {
//                left_round=1;
//                right_round=0;
//            }
//            if(SL_horizontal<SR_horizontal)
//            {
//                right_round=1;
//                left_round=0;
//            }
//        }
//
//    }
//
//}
//if(type_of_road==30&&into_the_ring==1)
//{
//    if(acc_encoder>short_theshold.round_SCFTM&&SMid_ad<short_theshold.cancel_ir)
//    {
//        type_of_road=0;
//        into_the_ring=0;
////            right_round=0;
////            left_round=0;
//        in_the_round=1;
//    }
//}
//if(in_the_round==1&&SMid_ad>short_theshold.round_midjudge)
//{
//
//      type_of_road=31;
//      acc_encoder=0;
//
//}
//if(type_of_road==31&&SMid_ad>short_theshold.out_flagjudge)
//{
//        out_the_ring=1;
//}
//
//if(out_the_ring==1&&type_of_road==31)
//{
//    if(acc_encoder>short_theshold.round_SCFTM&&SMid_ad<short_theshold.cancel_ir)
//    {
//        type_of_road=0;
//        in_the_round=0;
//        right_round=0;
//        left_round=0;
//        out_the_ring=0;
//    }
//
//}


void NNOM_process(void)
{
    Short_AD_Sample();
    Short_AD_normalization();
//    int8 test[7]={5,0.05,5.5,62,127,0.05,115};
//    int8 test[7]={34,1,41,127,73,1,53};
//    int8 test[7]={40,0.05,50,127,50,0.05,40};

}
// adc采集函数
void LV_Sample(void)
{
    if(short_process_flag==0)
    {
        Long_AD_Sample();
    }
    Short_AD_Sample();
}

void Long_AD_normalization(void)
{
    AD[0] = (135*Average[0])/Max[0];
    AD[1] = (135*Average[1])/Max[1];
    AD[2] = (140*Average[2])/Max[2];
    AD[3] = (140*Average[3])/Max[3];
    AD[4] = (125*Average[4])/Max[4];
    AD[5] = (125*Average[5])/Max[5];
    AD[6] = (127*Average[6])/Max[6];
    for(uint8 i= 0;i<AD_NUM;i++)
    {
        //暂时去掉归一化 AD[i] = (100*LV[i])/Max[i];//(K = 100)
//        AD[i] = (0.5*BIT_MAX*Average[i])/Max[i];
        if(AD[i]>BIT_MAX)
            AD[i]=BIT_MAX;
        //AD[i] = transfer[i];

    }
}

void Short_AD_normalization(void)
{
    AD[7] = (170*Average_AI[0])/Max[7];//(K = 100)
    AD[8] = (150*Average_AI[1])/Max[8];
    AD[9] = (150*Average_AI[2])/Max[9];
    AD[10] = (127*Average_AI[3])/Max[10];
    AD[11] = (127*Average_AI[4])/Max[11];
    AD[12] = (127*Average_AI[5])/Max[12];
    AD[13] = (127*Average_AI[6])/Max[13];
    AD[14] = (150*Average_AI[7])/Max[14];
    AD[15] = (150*Average_AI[8])/Max[15];
    AD[16] = (170*Average_AI[9])/Max[16];
    AD[17] = (127*Average_AI[10])/Max[17];
    AD[18] = (127*Average_AI[11])/Max[18];
    AD[19] = (127*Average_AI[12])/Max[19];
    AD[20] = (127*Average_AI[13])/Max[20];


//    for(uint8 i=AD_NUM;i<(AD_NUM+AI_NUM);i++)
//    {
//        AD[i] = (0.5*BIT_MAX*Average_AI[i-AD_NUM])/Max[i];
//        if(AD[i]>BIT_MAX)
//        {
//            AD[i]=BIT_MAX;
//        }
//    }
}


void Get_AI_AD (void)
{

       if (collect_max_flag!=1)
       {
           if(collect_data_finish==0)
           {
               if(short_process_flag==0)
               {
                   Long_AD_normalization();
               }
               Short_AD_normalization();
               //AD[i+AD_NUM] = transfer_AI[i];
               collect_data_finish=1;
           }
       }
       else if (collect_max_flag==1)
       {
           if (short_process_flag==0)
           {
               for(uint8 i=0;i<AD_NUM;i++)
               {
                   if(Average[i] > Max[i])
                   {
                     Max[i] = Average[i];
                   }
                   AD[i] = Average[i];
               }
           }
           for(uint8 i=0;i<AI_NUM;i++)
           {
               if(Average_AI[i] > Max[i+AD_NUM])
               {
                 Max[i+AD_NUM] = Average_AI[i];
               }
               AD[i+AD_NUM] = Average_AI[i];
           }

       }


}

void get_err(void)
{
    if(short_process_flag!=1)
    {
        //横电感差比积，右一减左一
        err_ad_now[0]=(AD[1]-AD[0])/(AD[1]*AD[0]+1);
        //竖横电感差比积，右减左+10减小竖电感差比积抖动
        err_ad_now[1]=(AD[3]-AD[2])/(AD[3]*AD[2]+10);
        //给竖电感较小的权重
        err_synthetical_now=0*err_ad_now[1]+1*err_ad_now[0];
    }
    if(short_process_flag==1)
    {
        //横电感差比积，右一减左一
//        err_ad_now[0]=(AD[16]-AD[7])/(AD[16]*AD[7]+1);
//右竖2 60度
//        err_ad_now[0]=(AD[13]-AD[10])/(AD[13]*AD[10]+1);
//        err_ad_now[0]=(AD[14]-AD[9])/(AD[14]*AD[9]+1);
//        err_ad_now[0]=(AD[12]-AD[10])/(AD[12]*AD[10]+1);
        //竖横电感差比积，右减左+10减小竖电感差比积抖动
//先试一下KP、KD
//        if(type_of_road==0)
//        {
//            err_ad_now[0]=(AD[16]-AD[7])/(AD[16]*AD[7]+1);
//        }else if(type_of_road==30)
//        {
//            err_ad_now[0]=(AD[14]-AD[9])/(AD[14]*AD[9]+1);
//        }

        err_ad_now[0]=(AD[16]-AD[7])/(AD[16]*AD[7]+1);
////再试一下
//        err_ad_now[0]=(AD[16]-0.2*AD[7])/(AD[16]*0.2*AD[7]+1);



        err_ad_now[1]=(AD[15]-AD[8])/(AD[8]*AD[15]+4);
        //给竖电感较小的权重
        err_synthetical_now=0*err_ad_now[1]+1*err_ad_now[0];
//        err_synthetical_now=0*err_ad_now[1]+1*err_ad_now[0];
    }


}


/**********************************************************************************************************************
*  @brief      电磁识别道路类型
*  @since      v1.1
*  Sample usage:  0      直道+45度转角
*                 10/11  90度转角左、右打死
*                 20/21  20左环、21右环
**********************************************************************************************************************/
void recognize_road(void)
{
    if (short_process_flag==0)
    {
        Long_recognize_road();
    }
    else if(short_process_flag==1)
    {
        Short_recognize_road();
    }

}

void AI_recognize_road(void)
{

    float AIL_vertical,AIR_vertical,AIR_horizontal,AIL_horizontal,AIMid_ad,AIL_vertical_2,AIR_vertical_2;
    AIL_vertical=AD[8];
    AIR_vertical=AD[15];
    AIMid_ad=AD[12];
    AIL_horizontal=AD[7];
    AIR_horizontal=AD[16];
    AIL_vertical_2=AD[10];
    AIR_vertical_2=AD[13];
    if(AIMid_ad+AIL_horizontal+AIR_horizontal>AI_theshold.round_mid&&acc_record_distance>190000)
    {
        acc_record_distance=0;
        type_of_road=30;
        if(right_round==0&&left_round==0)
        {
            if(AIL_vertical_2>AIR_vertical_2)
            {
                left_round=1;
                right_round=0;
                intergration = 0.0f;
                into_the_ring=1;
            }
            else if(AIL_vertical_2<AIR_vertical_2)
            {
                right_round=1;
                left_round=0;
                intergration = 0.0f;
                into_the_ring=1;
            }
        }
    }

    if(type_of_road==30)
    {
        if((intergration>=AI_theshold.angle||intergration<=-AI_theshold.angle)||AIMid_ad+AIL_horizontal+AIR_horizontal<AI_theshold.cancel_into_round)
        {
            type_of_road=0;
            into_the_ring=0;
            in_the_round=1;
            right_round=0;
            left_round=0;
        }

    }
}
/**********************************************************************************************************************
*  @brief      电磁识别道路类型
*  @since      v1.1
*  Sample usage:  0      直道+45度转角
*                 10/11  90度转角左、右打死
*                 20/21  20左环、21右环
**********************************************************************************************************************/
void Long_recognize_road(void)
{
    //直角弯标志位
    float L_vertical,R_vertical,L_horizontal,R_horizontal,Mid_ad,L_45Curve,R_45Curve;
    L_horizontal=AD[0];
    R_horizontal=AD[1];
    L_vertical=AD[2];
    R_vertical=AD[3];
//    //用于区分45度角和直角
//    L_45Curve=AD[4];
//    R_45Curve=AD[5];
    //中间横电感
    Mid_ad=AD[6];

    if (R_vertical-L_vertical>long_theshold.turn_90_curve&&L_vertical<long_theshold.is_cross_not_90&&L_horizontal+R_horizontal<long_theshold.is_45_not_90)
    {
        type_of_road=11;
        acc_encoder=0;
    }
    if (L_vertical-R_vertical>long_theshold.turn_90_curve&&R_vertical<long_theshold.is_cross_not_90&&L_horizontal+R_horizontal<long_theshold.is_45_not_90)
    {
        type_of_road=10;
        acc_encoder=0;
    }

//    if(type_of_road!=21)
//    {
//        if (Mid_ad>=long_theshold.round_midjudge)
//        {
//            type_of_road=21;
//            acc_encoder=0;
//        }
//    }
////    if(type_of_road!=20)
////    {
////        if (AD[6]>=200&&AD[0]-AD[1]<-10)
////        {
////            type_of_road=21;
////        }
////    }
////    清除直角弯标志位

    if(type_of_road==0)
    {
       if(R_vertical>long_theshold.cross_pid_ad&&L_vertical>long_theshold.cross_pid_ad&&Mid_ad>100.0f)
       {
           cross_pid_change_flag=1;
       }else
       {
           cross_pid_change_flag=0;
       }
    }
    else if (type_of_road==11||type_of_road==10)
    {
        if (Mid_ad>long_theshold.cancel_90_status&&acc_encoder>long_theshold.encoder_90_num)
        {
            type_of_road=0;
        }
    }
//    if (type_of_road==20||type_of_road==21)
//    {
//        if (AD[6]<=130&&AD[1]+AD[0]<100&&acc_encoder>long_theshold._SCFTM)
//        {
//            type_of_road=0;
//        }
//    }
}

/**********************************************************************************************************************
*  @brief      电磁识别道路类型
*  @since      v1.1
*  Sample usage:  0      直道+45度转角
*                 10/11  90度转角左、右打死
*                 20/21  20左环、21右环
**********************************************************************************************************************/
void Short_recognize_road(void)
{

    float SL_horizontal,SR_horizontal,SMid_ad,SL_vertical_2,SR_vertical_2;
//    SL_vertical=AD[8];
//    SR_vertical=AD[15];SL_vertical,SR_vertical,
    SMid_ad=AD[12];
    SL_horizontal=AD[7];
    SR_horizontal=AD[16];
    SL_vertical_2=AD[10];
    SR_vertical_2=AD[13];
    num_of_three_horizen= SL_horizontal+SR_horizontal+SMid_ad;
    if(num_of_three_horizen>up_speed_ad)
    {
        up_speed_flag=1;
    }else
    {
        up_speed_flag=0;
    }
    if(SMid_ad+SL_horizontal+SR_horizontal>short_theshold.round_mid&&acc_record_distance>250000)
    {
        in_the_round=1;
        acc_record_distance=0;
        type_of_road=30;
        if(right_round==0&&left_round==0)
        {
            //强制进环岛
//            if(round_num<2)
//            {
                if(SL_vertical_2>SR_vertical_2)
                {
                    left_round=1;
                    round_num++;
                    right_round=0;
                    intergration = 0.0f;
                    round_achive_angle=0.0f;
                    into_the_ring=1;
                }
                else if(SL_vertical_2<SR_vertical_2)
                {
                    right_round=1;
                    round_num++;
                    left_round=0;
                    intergration = 0.0f;
                    round_achive_angle=0.0f;
                    into_the_ring=1;
                }
//            }else
//            {
//                right_round=1;
//                round_num++;
//                left_round=0;
//                intergration = 0.0f;
//                round_achive_angle=0.0f;
//                into_the_ring=1;
//            }


        }

    }
    if(type_of_road==30)
    {
        if((intergration>=short_theshold.angle||intergration<=-short_theshold.angle)||SMid_ad+SL_horizontal+SR_horizontal<short_theshold.cancel_into_round)
        {
            //短前瞻跑圆环
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 0)
            type_of_road=41;
#endif
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 1)
//AI跑圆环
            type_of_road=0;
#endif
            into_the_ring=0;
            in_the_round=1;

        }

    }
//AI跑圆环
#if defined  (AI_YUANHUAN_BUTTON) && (AI_YUANHUAN_BUTTON == 0)
    else if(type_of_road==41)
    {
        err_round_ai=(SR_horizontal-SL_horizontal)/(SL_horizontal*SR_horizontal+1);
        if(round_achive_angle>AI_angle.out_round_angle||round_achive_angle<-AI_angle.out_round_angle)
        {
//            in_the_round=0;

            out_round_speed=0;
            type_of_road=0;
            right_round=0;
            left_round=0;
        }
    }
#endif
    //检测圆环
//  if(type_of_road==41)
//  {
//      GPIO_Set(P20,10, 1);
//  }else
//  {
//      GPIO_Set(P20,10, 0);
//  }
//    if(in_the_round==1)
//    {
//        if(SMid_ad+SL_horizontal+SR_horizontal>short_theshold.round_mid)
//        {
//
//        }
//    }

// 圆环v1.0
//
//        if (in_the_round==0&&SMid_ad>short_theshold.round_mid)
//        {
//           type_of_road=30;
//        }
//        if(type_of_road==30)
//        {
//            if(SMid_ad>short_theshold.into_round)
//            {
//                if(right_round==0&&left_round==0)
//                {
//                    if(SL_vertical> SR_vertical)
//                    {
//                        left_round=1;
//                        right_round=0;
//                        intergration = 0.0f;
//                    }
//                    else if(SL_vertical< SR_vertical)
//                    {
//                        right_round=1;
//                        left_round=0;
//                        intergration = 0.0f;
//                    }
//                }
//                into_the_ring=1;
//            }
//        }
//            if(type_of_road==30&&into_the_ring==1)
//            {
//                if(intergration>=short_theshold.angle||intergration<=-AI_theshold.angle)
//                {
//                    type_of_road=0;//环内是用AI去控制还是传统区去控制
//                    into_the_ring=0;
//                    in_the_round=1;
//                }
//            }
//                if(in_the_round==1&&SMid_ad>short_theshold.round_out_mid)
//                {
//
//                      type_of_road=31;
//                      acc_encoder=0;
//
//                }
//                if(type_of_road==31&&SMid_ad>short_theshold.out_round)
//                {
//                     if(out_the_ring==0)
//                     {
//                         intergration = 0.0f;
//                     }
//                    out_the_ring=1;
//                }
//
//                if(out_the_ring==1&&type_of_road==31&&(intergration>=AI_theshold.angle_out||intergration<=-AI_theshold.angle_out))
//                {
//                        in_the_round=0;
//                        right_round=0;
//                        left_round=0;
//                        out_the_ring=0;
//                        Repeated_recognition_flag = 0;
//                }
//
//    if(type_of_road!=30&&type_of_road!=31)
//    {
//        //if(SL_vertical-SR_vertical>short_theshold.turn_90_curve&&SR_vertical<short_theshold.is_cross_not_90&&SL_horizontal+SR_horizontal<short_theshold.round_misjudge_num&&SL_horizontal<short_theshold.round_misjudge_single)
////      没必要减小误判因为45度只要不减速误判也没什么,加入中间横电感防止圆环对直角误判
//        if(SL_vertical-SR_vertical>short_theshold.turn_90_curve&&SR_vertical<short_theshold.is_cross_not_90&&SMid_ad<short_theshold.is_round_not_90_mid)
//        {
//            type_of_road=10;
//            acc_encoder=0;
//        }
//        if(SR_vertical-SL_vertical>short_theshold.turn_90_curve&&SL_vertical<short_theshold.is_cross_not_90&&SMid_ad<short_theshold.is_round_not_90_mid)
//        {
//            type_of_road=11;
//            acc_encoder=0;
//        }
//    }
//
//    if (type_of_road==11||type_of_road==10)
//    {
//        if (SMid_ad>short_theshold.cancel_90_status && acc_encoder>short_theshold.encoder_90_num)
//        {
//            type_of_road=0;
//        }
//    }
}


//状态判别法已弃用
////    for(uint8 i=0;i<buffer_num;i++)
////    {
////        AD_SLV_LAST[i]=AD_SLV_LAST[i+1];
////    }
////    AD_SLV_LAST[buffer_num-1]=SL_vertical;
////    uint8 lh_grow_high=0,grow_low=0;
////    for(uint8 h=1;h<15;h++)
////    {
////        if (AD_SLV_LAST[h+15]-AD_SLV_LAST[h]>1)//0.5是否合理需要改
////        {
////            lh_grow_high++;
////        }
//////        else if(AD_SLV_LAST[h+15]-AD_SLV_LAST[h-15]<-1)
//////        {
//////            grow_low++;
//////        }
////    }
//
//    for(uint8 i=0;i<buffer_num;i++)
//    {
//        AD_SLH_LAST[i]=AD_SLH_LAST[i+1];
//    }
//    AD_SLH_LAST[buffer_num-1]=SL_horizontal;
//    uint8 sl_growlow=0,grow_low=0;
//    for(uint8 h=1;h<15;h++)
//    {
//        if (AD_SLH_LAST[h+15]-AD_SLH_LAST[h]<-0.2)//0.5是否合理需要改
//        {
//            sl_growlow++;
//        }
////        else if(AD_SLV_LAST[h+15]-AD_SLV_LAST[h-15]<-1)
////        {
////            grow_low++;
////        }
//    }
/**********************************************************************************************************************
*  @brief      出赛道保护
*  @since      v1.1
*
**********************************************************************************************************************/
void out_of_road(void)
{
    if(type_of_road==0)
    {
        uint8 count=0;
        for(uint8 i= 8;i<=14;i++)
        {
             if (AD[i]<=1)
             {
                  count++;
             }
        }
        if(count>=20)
        {
           If_Start = 0 ;
        }
    }

}


//void Send_tesst(void)
//{
//    if(send_data_flag)
//    {
////        //两个time
////        send_test[0]=0;
////        send_test[1]=0;
//        //十四个AI电感
//        for(uint8 i=0;i<10;i++)
//        {
//            send_test[i]=(uint8)((int8)((int32)AD[i+7]-128));
//        }
//        send_test[10]=0;
//        send_test[11]=0;
//        send_test[12]=0;
//        send_test[13]=0;
//        //电机和舵机
//        send_test[14]=(uint8)((int8)((int32)(127*((pwm_servo-servo_mid)/1.8))));
//        send_test[15]=0;//电机
////        send_test[12]=(uint8)((int8)(int32)(128*(pwm_moto/70)));
////        for(uint8 k=0;k<7;k++)
////        {
////            send_test[(k+13)]=0;
////        }
//        send_test[16]=0x5a;
//        SmartCar_Uart_Transfer(send_test,17,0);
//        send_data_flag=0;
//    }
//}

void Send_train_data(void)
{
    if(send_data_flag)
    {
        if(collect_data_finish)
        {
            //14个AI电感
            //1个舵机
            //1个电机
            //1个标志位0x5a
            //17个数据
            for(uint8 i=0;i<14;i++)
            {
                send_test[i]=(uint8)((int8)((int32)AD[i+7]-128));
            }
            send_test[3]=0;
            send_test[4]=0;
            send_test[6]=0;
//            send_test[10]=0;
//            send_test[11]=0;
//            send_test[12]=0;
//            send_test[13]=0;
            //电机和舵机
            send_test[14]=(uint8)((int8)((int32)(127*((pwm_servo-servo_mid)/1.7))));
            send_test[15]=0;//电机
    //        send_test[12]=(uint8)((int8)(int32)(128*(pwm_moto/70)));
    //        for(uint8 k=0;k<7;k++)
    //        {
    //            send_test[(k+13)]=0;
    //        }
            send_test[16]=0x5a;
            SmartCar_Uart_Transfer(send_test,17,2);
            collect_data_finish=0;
        }
    }else if(send_data_flag!=1)
    {
        if(collect_data_finish)
        {
            collect_data_finish=0;
        }
    }

}


void send_ad(void)
{


//观察短前瞻AD的值
#if defined  (SEND_AD_NUM_BUTTON) && (SEND_AD_NUM_BUTTON == 1)
        float sen_ad_1[20]={0};
        for(uint8 i=0;i<10;i++)
         {
             sen_ad_1[i]=AD[i+7];
         }

        sen_ad_1[10]=AD[7]+AD[16]+AD[12];
//        sen_ad_1[11]=AD[7]+AD[16];
//        sen_ad_1[12]=(AD[8]-AD[11])/(AD[7]+AD[16]+AD[12]);
       SmartCar_VarUpload(sen_ad_1,13);
#endif
//观察长前瞻AD的值
#if defined  (SEND_AD_NUM_BUTTON) && (SEND_AD_NUM_BUTTON == 2)
            float sen_ad_1[20]={0};
            for(uint8 i=0;i<7;i++)
             {
                 sen_ad_1[i]=AD[i];
             }

            sen_ad_1[10]=AD[0]+AD[1]+AD[6];
            sen_ad_1[11]=AD[0]+AD[1];
            sen_ad_1[12]=AD[4]+AD[5]+AD[6];
            sen_ad_1[13]=AD[4]+AD[5];
//            sen_ad_1[12]=AD[4]/AD[5];
//            sen_ad_1[13]=AD[5]/AD[4];

//            sen_ad_1[12]=(AD[8]-AD[11])/(AD[7]+AD[16]+AD[12]);
           SmartCar_VarUpload(sen_ad_1,14);
#endif

}


//AD处理程序分区
void Elec_process(void)
{
    LV_Sample();
    Get_AI_AD();
    recognize_road();
    get_err();
    Send_train_data();
#if defined  (SEND_AD_NUM_BUTTON) && (SEND_AD_NUM_BUTTON != 0)
    send_ad();
#endif
//    out_of_road();
//    if(send_ai_ad==1)
//    {
//    Send_Data();
//    Send_tesst();
//    }
//
//    if(short_control==1)
//    {
//    send_ad();
//        Send_Data();
//    }

}



